﻿#include "quaternion.h"

using namespace CAGD;

Quaternion::Quaternion(double s, BVec v) : m_s(s), m_v(v)
{
}

Quaternion::Quaternion(BVec v, double theta) : m_s(cos(theta / 2)), m_v(v.Normalized() * sin(theta / 2))
{
}

Quaternion::Quaternion(BVec a, BVec b)
{
    // 确保两个轴长度不为零
    assert(a.Modulus() > FLT_MIN && b.Modulus() > FLT_MIN);

    // 先把 a 转动到 z 轴：先绕 z 轴转，然后绕 y 轴转
    Quaternion q1(1, {0, 0, 0}), q2(1, {0, 0, 0});
    float proj = sqrt(pow(a.X(), 2) + pow(a.Y(), 2));

    // 如果已经在 z 轴，就不动；否则计算两个四元数
    if (proj > FLT_MIN)
    {
        q1 = Quaternion({0, 0, 1}, -acos(a.X() / proj));
        q2 = Quaternion({0, 1, 0}, -acos(a.Z() / a.Modulus()));
    }

    // 然后把 b 同样转动到 z 轴
    Quaternion q3(1, {0, 0, 0}), q4(1, {0, 0, 0});
    proj = sqrt(pow(b.X(), 2) + pow(b.Y(), 2));

    // 如果已经在 z 轴，就不动；否则计算两个四元数
    if (proj > FLT_MIN)
    {
        q3 = Quaternion({0, 0, 1}, -acos(b.X() / proj));
        q4 = Quaternion({0, 1, 0}, -acos(b.Z() / b.Modulus()));
    }

    // 最后初始化
    *this = Quaternion(1, {0, 0, 0}) / q3 / q4 * q2 * q1;
}

Quaternion Quaternion::operator*(double t) const
{
    return Quaternion(m_s * t, m_v * t);
}

Quaternion Quaternion::operator/(double t) const
{
    assert(fabs(t) > FLT_MIN);
    return Quaternion(m_s / t, m_v / t);
}

Quaternion Quaternion::operator+(const Quaternion &q) const
{
    return Quaternion(m_s + q.S(), m_v + q.V());
}

Quaternion Quaternion::operator-(const Quaternion &q) const
{
    return Quaternion(m_s - q.S(), m_v - q.V());
}

Quaternion Quaternion::operator*(const Quaternion &q) const
{
    return Quaternion(m_s * q.S() - m_v * q.V(), q.V() * m_s + m_v * q.S() + m_v % q.V());
}

Quaternion Quaternion::operator/(const Quaternion &q) const
{
    return (*this) * Quaternion(q.S(), q.V() * -1) / pow(sqrt(q.S() * q.S() + pow(q.V().Modulus(), 2)), 2);
}

bool Quaternion::operator==(const Quaternion &q) const
{
    return (q - *this).Modulus() < CAGD_ABS_ACC;
}

void Quaternion::Normalize()
{
    double len = Modulus();
    assert(len > FLT_MIN);
    m_s /= len;
    m_v = m_v / len;
}

Quaternion Quaternion::Normalized() const
{
    double len = Modulus();
    assert(len > FLT_MIN);
    return *this / len;
}

double Quaternion::Modulus() const
{
    return sqrt(SquareModulus());
}

double Quaternion::SquareModulus() const
{
    return m_s * m_s + m_v.SquareModulus();
}

double Quaternion::S() const
{
    return m_s;
}

void Quaternion::SetS(double s)
{
    m_s = s;
}

BVec Quaternion::V() const
{
    return m_v;
}

void Quaternion::SetV(const BVec &v)
{
    m_v = v;
}